﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using Trio.Motor;
using System.Timers;
using Atc600Control;

namespace RollController2022
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }
        System.Timers.Timer getvalue_timer = new System.Timers.Timer();//位置速度取值定时器

        int move_axis = 10;//轴10为左电机；
        int back_axis = 3; //轴3为右电机；
        int Encoder_axis = 0; //编码器轴0；
        int Tension_axis = 9; //左磁粉DA9；
        TrioP600 trio = new TrioP600();
        Atc600 atc600 = new Atc600();
        private void Form1_Load(object sender, EventArgs e)//From加载
        {
            SetMode();//模式选择显示隐藏输入框
            trio.Connect();//通讯连接
            trio.RollInitMpos(Encoder_axis);//编码器清零
            atc600.Connect();//通讯连接
            trio.SetTension(Tension_axis, 1);//左磁粉初始1N
            atc600.SetTension(0, 1);//右磁粉手动值1N
            getvalue_timer.Interval = 50;
            getvalue_timer.Elapsed += new ElapsedEventHandler(value_Elapsed);//PID、读数
        }

        private void Form1_FormClosed(object sender, FormClosedEventArgs e)//Form关闭
        {
            timer1.Stop();//停止显示
            getvalue_timer.Stop();//停止读取
            trio.Closed();//停止运动，断开连接
            atc600.Closed();//断开连接
        }

        private void btn_Clear_Click(object sender, EventArgs e)
        {
            rbtn_Circle.Enabled = true;
            rbtn_Runway.Enabled = true;
            rbtn_Back.Enabled = true;

            cb_LReverse.Enabled = true;
            cb_RReverse.Enabled = true;

            cb_Length.Enabled = true;
            cb_Turns.Enabled = true;
            tbx_Length.Enabled = true;
            tbx_Turns.Enabled = true;

            btn_L.Enabled = true;
            btn_R.Enabled = true;

            btn_Clear.Enabled = true;

            lb_Speed.Text = "0.00";//速度清零
            lb_Length.Text = "0.00";//长度显示清零
            lb_Turns.Text = "0.0";//圈数显示清零
            btn_Start.Text = "Start";
        }

        private void cb_Length_Click(object sender, EventArgs e)//定长限制，单选
        {
            if (cb_Length.Checked == true)
            {
                cb_Turns.Checked = false;
            }
        }
        private void cb_Turns_Click(object sender, EventArgs e)//圈数限制，单选
        {
            if (cb_Turns.Checked == true)
            {
                cb_Length.Checked = false;
            }
        }

        private void btn_Start_Click(object sender, EventArgs e)
        {
            if (btn_Start.Text == "Start")
            {
                cb_LReverse.Enabled = false;
                cb_RReverse.Enabled = false;

                rbtn_Circle.Enabled = false;
                rbtn_Runway.Enabled = false;
                rbtn_Back.Enabled = false;

                cb_Length.Enabled = false;
                cb_Turns.Enabled = false;
                tbx_Length.Enabled = false;
                tbx_Turns.Enabled = false;

                btn_L.Enabled = false;
                btn_R.Enabled = false;
                btn_Clear.Enabled = false;

                trio.RollInitMpos(Encoder_axis);

                SetMove();//轴方向，左右磁粉
                lasterror = error = piterm = diterm = 0;//PID清零
                getvalue_timer.Start();//打开定时器
                timer1.Start();//开始显示

                btn_Start.Text = "Stop";
            }
            else if (btn_Start.Text == "Continue")
            {
                btn_Clear.Enabled = false;
                tbx_Length.Enabled = false;
                tbx_Turns.Enabled = false;

                SetMove();//轴方向，左右磁粉
                lasterror = error = piterm = diterm = 0;//PID清零
                getvalue_timer.Start();//打开定时器
                timer1.Start();//开始显示

                btn_Start.Text = "Stop";
            }
            else if (btn_Start.Text == "Stop")
            {
                btn_Clear.Enabled = true;
                if (cb_Length.Checked == true) tbx_Length.Enabled = true;
                if (cb_Turns.Checked == true) tbx_Turns.Enabled = true;

                timer1.Stop();//停止显示
                getvalue_timer.Stop();//停止读取控制
                trio.StopMove();//停止运动
                trio.SetTension(Tension_axis, 1);//左磁粉初始1N
                atc600.SetTension(0, 1);//右磁粉手动值1N
                btn_Start.Text = "Continue";
            }
        }

        double speed_set, angle_set, backspeed_set;
        double length_read, speed_read, angle_read;
        DateTime dt_last;
        TimeSpan Time;
        double Encoder, Encoder_last;
        private void value_Elapsed(object sender, ElapsedEventArgs e)//timer
        {
            Encoder_last = Encoder;//储存编码器
            Encoder = Math.Abs(trio.GetEncoder(Encoder_axis));//编码器值，轴0

            length_read = Encoder / 8000.0 * 0.16 * 3.1415926;//编码器换算总长度m
            angle_read = Encoder / 8000;//计算圈数

            DateTime dt_now = DateTime.Now;
            Time = dt_now.Subtract(dt_last);

            speed_read = Math.Abs(Encoder_last - Encoder) / 8000.0 * 0.16 * 3.1415926 / Time.TotalMilliseconds * 1000;//计算速度m/s
            dt_last = dt_now;

            if (rbtn_Circle.Checked == true)//圆，恒线
            {
                trio.CtrlSpeed(move_axis, PidSpeed(speed_set, speed_read));//左侧轴10
                CheckLimit();
            }
            else if (rbtn_Runway.Checked == true)//跑道，恒角
            {
                trio.CtrlSpeed(move_axis, angle_set * 8000);//rps
                CheckLimit();
            }
            else if (rbtn_Back.Checked == true)
            {
                trio.CtrlSpeed(back_axis, PidSpeed(backspeed_set, speed_read));//右侧轴3
            }
        }

        private void CheckLimit()
        {
            if (cb_Length.Checked == true)//长度限制
            {
                if (Convert.ToDouble(length_read) > Convert.ToDouble(tbx_Length.Text))
                {
                    btn_Start.PerformClick();
                    MessageBox.Show("");
                }
            }
            else if (cb_Turns.Checked == true)//圈数限制
            {
                if (Convert.ToDouble(angle_read) > Convert.ToDouble(tbx_Turns.Text))
                {
                    btn_Start.PerformClick();
                    MessageBox.Show("");
                }
            }
        }

        DateTime PID_lasttime;
        TimeSpan PID_changetime;
        float Kp = 1, Kd = 18;
        float lasterror, error, piterm, diterm;
        private double PidSpeed(double set, double read)
        {
            DateTime dt_now = DateTime.Now;
            PID_changetime = dt_now.Subtract(PID_lasttime);
            PID_lasttime = dt_now;

            double output_V;

            error = (float)(set - read);
            piterm = Kp * error;
            diterm = (float)(Kd * (error - lasterror) * 1000 / PID_changetime.TotalMilliseconds);

            output_V = set / 0.16 / 3.1415926 * 8000 * 0.9 + (piterm + diterm);

            if (output_V < 0)
            {
                output_V = 0;
            }
            if (output_V > 2000)
            {
                output_V = 2000;
            }
            lasterror = error;
            return output_V;
        }

        private void timer1_Tick(object sender, EventArgs e)//显示
        {
            lb_Speed.Text = Math.Round(speed_read, 2).ToString();
            lb_Length.Text = Math.Round(length_read, 2).ToString();
            lb_Turns.Text = Math.Round(angle_read, 1).ToString();
        }

        private void btn_L_MouseDown(object sender, MouseEventArgs e)
        {
            int dir = 0;
            if (cb_LReverse.Checked == true) dir = 0;
            else if (cb_LReverse.Checked == false) dir = 1;

            trio.RoollInitMotor(move_axis, dir);//设置轴与方向

            trio.SetTension(Tension_axis, 50);//左张力50N
            atc600.SetTension(1, 1);//右张力设置1N
            trio.CtrlSpeed(move_axis, 500);
        }

        private void btn_L_MouseUp(object sender, MouseEventArgs e)
        {
            trio.CtrlSpeed(move_axis, 0);

            trio.StopMove();
            trio.SetTension(Tension_axis, 1);//左磁粉初始1N
            atc600.SetTension(0, 1);//右磁粉手动值1N
        }

        private void btn_R_MouseDown(object sender, MouseEventArgs e)
        {
            int dir = 0;
            if (cb_LReverse.Checked == true) dir = 0;
            else if (cb_LReverse.Checked == false) dir = 1;

            trio.RoollInitMotor(back_axis, dir);//设置轴与方向

            trio.SetTension(Tension_axis, 1);//左张力1N
            atc600.SetTension(0, 50);//右张力设置50N
            trio.CtrlSpeed(back_axis, 500);
        }
        private void btn_R_MouseUp(object sender, MouseEventArgs e)
        {
            trio.CtrlSpeed(back_axis, 0);

            trio.StopMove();
            trio.SetTension(Tension_axis, 1);//左磁粉初始1N
            atc600.SetTension(0, 1);//右磁粉手动值1N
        }

        private void rbtn_Circle_CheckedChanged(object sender, EventArgs e)
        {
            SetMode();
        }

        private void rbtn_Runway_CheckedChanged(object sender, EventArgs e)
        {
            SetMode();
        }

        private void rbtn_Back_CheckedChanged(object sender, EventArgs e)
        {
            SetMode();
        }

        private void numUpD_LTorque_ValueChanged(object sender, EventArgs e)//左扭矩
        {
            if (btn_Start.Text == "Stop")
            {
                trio.SetTension(Tension_axis, (double)numUpD_LTorque.Value);
            }
        }

        private void numUpD_RTorque_ValueChanged(object sender, EventArgs e)//右张力
        {
            if (btn_Start.Text == "Stop")
            {
                atc600.SetTension(1, (int)numUpD_RTorque.Value);//右张力设置
            }
        }

        private void SetMove()
        {
            speed_set = Math.Round((double)numUpD_LSpeed.Value, 2);//m/s
            angle_set = Math.Round((double)numUpD_LAngle.Value, 1);//rpm
            backspeed_set = Math.Round((double)numUpD_RSpeed.Value, 2);//m/s
            int L_dir = 0, R_dir = 0;
            if (cb_LReverse.Checked == true) L_dir = 1;
            else if (cb_LReverse.Checked == false) L_dir = 0;

            if (cb_RReverse.Checked == true) R_dir = 1;
            else if (cb_RReverse.Checked == false) R_dir = 0;

            if (rbtn_Circle.Checked == true)//圆形,恒线
            {
                trio.RoollInitMotor(move_axis, L_dir);//设置主动轴10与方向
                trio.SetTension(Tension_axis, 50);//左张力50V
                atc600.SetTension(1, (int)numUpD_RTorque.Value);//右张力设置
            }
            else if (rbtn_Runway.Checked == true)//跑道，恒角
            {
                trio.RoollInitMotor(move_axis, L_dir);//设置轴与方向
                trio.SetTension(Tension_axis, 50);//左张力10V，
                atc600.SetTension(1, (int)numUpD_RTorque.Value);//右张力设置
            }
            else if (rbtn_Back.Checked == true)//退回
            {
                trio.RoollInitMotor(back_axis, R_dir);//设置轴与方向
                trio.SetTension(Tension_axis, (double)numUpD_LTorque.Value);//左张力设置
                atc600.SetTension(0, 50);//右张力10V，非恒张力
            }
        }

        private void SetMode()
        {
            if (rbtn_Circle.Checked == true)//圆形,恒线
            {
                numUpD_LTorque.Visible = false;//左张力
                lb_LTorque.Visible = false;
                numUpD_LAngle.Visible = false;//左角速度
                lb_LAngle.Visible = false;
                numUpD_LSpeed.Visible = true;//左线速度
                lb_LSpeed.Visible = true;
                numUpD_RTorque.Visible = true;//右张力
                lb_RTorque.Visible = true;
                numUpD_RSpeed.Visible = false;//右线速度
                lb_RSpeed.Visible = false;
            }
            else if (rbtn_Runway.Checked == true)//跑道，恒角
            {
                numUpD_LTorque.Visible = false;//左张力
                lb_LTorque.Visible = false;
                numUpD_LAngle.Visible = true;//左角速度
                lb_LAngle.Visible = true;
                numUpD_LSpeed.Visible = false;//左线速度
                lb_LSpeed.Visible = false;
                numUpD_RTorque.Visible = true;//右张力
                lb_RTorque.Visible = true;
                numUpD_RSpeed.Visible = false;//右线速度
                lb_RSpeed.Visible = false;
            }
            else if (rbtn_Back.Checked == true)//退回
            {
                numUpD_LTorque.Visible = true;//左张力
                lb_LTorque.Visible = true;
                numUpD_LAngle.Visible = false;//左角速度
                lb_LAngle.Visible = false;
                numUpD_LSpeed.Visible = false;//左线速度
                lb_LSpeed.Visible = false;
                numUpD_RTorque.Visible = false;//右张力
                lb_RTorque.Visible = false;
                numUpD_RSpeed.Visible = true;//右线速度
                lb_RSpeed.Visible = true;
            }
        }
    }
}
